
from visual import *
from robot import cc
from Brain.vector import Vector
from Brain.plan import MovementStep, BuildStep, PlacelintelStep, FindpatternStep, ChasepakStep, ApproachsiteStep

import time

class HistoryVisualisation:
	""" Visual Utility 
	"""
	TRAIL_COLOR = (cc(120),cc(190),cc(169))
	POINTED_COLOR = (cc(191),cc(119),cc(130))
	Z_DISTANCE = 40
	TRAIL_WIDTH = 10
	TRAIL_HEIGHT = 350
	OPACITY = 0.2
	YPOSITIONS = [-40, -20, 20, 40,]
	
	def __init__(self):
		self._movement_trail = []
		self._endpoints = []
		# current point
		self._pointed = box(pos=(0,0,0), length=10, height=self.TRAIL_HEIGHT, width=self.TRAIL_WIDTH, color=self.POINTED_COLOR)
		self._pointed.opacity = self.OPACITY
		self._pointed.visible = 0
		self._show_trail = True
		self._show_pointer = False
		self._current_direction = None
		self._labelpositions_index = 0

	def toggle_pointer(self):
		""" display/hide pointer trail """
		self._show_pointer = not self._show_pointer
		if self._show_pointer == False: self._pointed.visible = 0
	
	def toggle_trail(self):
		""" display/hide plan trail """
		self._show_trail = not self._show_trail
		if self._show_trail == False: 
			for x in self._movement_trail:
				x.visible = 0
	
#	def get_current_angle(self):
#		return self._current_angle
	
	def get_current_direction(self):
		return self._current_direction
	
	def update(self, plan):
		for x in self._movement_trail: x.visible = 0
		self._movement_trail = []
		self._endpoints = []
		path = plan.move_path()
		old_point = None
		
		for step in path:
			if old_point: coordinates = (old_point.x, old_point.y)
			else: coordinates = (0, 0)

			if isinstance(step, Vector):
				self._endpoints.append(step)
				if old_point:
					direction = step - old_point
					position = old_point + (direction / 2.0)
					position = position.export()
					position.z = self.Z_DISTANCE
					trail_step = box(	pos=position, axis=direction.export(),
										length=direction.length(), height=self.TRAIL_HEIGHT, 
										width=self.TRAIL_WIDTH, color=self.TRAIL_COLOR	)
					trail_step.opacity = self.OPACITY
					if not self._show_trail: trail_step.visible = 0
					self._movement_trail.append( trail_step )
				old_point = step
			
			elif isinstance(step, (BuildStep, PlacelintelStep, ChasepakStep, FindpatternStep, )):
				trail_step = label(xoffset=40, yoffset=self.getyoffset(), height=12, border=10, space=15, 
					text=str(step), pos=coordinates)
				if not self._show_trail: trail_step.visible = 0
				self._movement_trail.append( trail_step )
			
			elif isinstance(step, ApproachsiteStep):
				trail_step = label(xoffset=40, yoffset=self.getyoffset(), height=12, border=10, space=15, 
					text=str(step), pos=coordinates)
				if not self._show_trail: trail_step.visible = 0
				self._movement_trail.append( trail_step )

		self._labelpositions_index = 0
	
	def update_pointer(self, point):
		""" point is tuple """
		if len(self._endpoints) > 0:
			self._pointed.visible = 1
			frm = self._endpoints[-1]
			direction = point - frm
			self._current_direction = direction
			if self._show_pointer:
				position = frm + (direction / 2.0)
				position = position.export()
				position.z = self.Z_DISTANCE
				self._pointed.pos = position
				self._pointed.axis = direction.export()
				self._pointed.length = direction.length()
			return
		self._pointed.visible = 0

	def getyoffset(self):
		if self._labelpositions_index >= len(self.YPOSITIONS):
			self._labelpositions_index = 0
		ret = self.YPOSITIONS[self._labelpositions_index]
		self._labelpositions_index += 1
		return ret







